/*
 * Selector.cpp
 *
 *  Created on: 24 янв. 2013
 *      Author: alex
 */

#include "Selector.h"
#include <cmath>
Selector* Selector::_instance=0;
#include "ClientSocket.h"
extern ClientSocket *g_socket;


Selector::Selector():terr(new TerritorialityModule()),expl(new ExplorationModule()),nest(new NestModule()),step(0)
{

}

Selector::~Selector()
{
	delete terr;
	delete expl;
	delete nest;
}

Selector* Selector::instance()
{
	if(_instance==0)
		_instance = new Selector();
	return _instance;
}

#define EXPL 0.25
#define NESTSTEPS 25

Tile Selector::getDecision()
{
	if(Controller::instance()->getTile() == Controller::instance()->start())
			{
				if(step == NESTSTEPS)
				{

					(*g_socket)<<"nest"<<Controller::instance()->start().x<<" "<<Controller::instance()->start().y;
				}
				step = 0;

			}

	printf("terr...\n");
	Tile t=terr->getDecision();
	printf("expl...\n");
	Tile e=expl->getDecision();
	printf("nest...\n");
	Tile n=nest->getDecision();

	double nestProb = pow((double)step++/NESTSTEPS,4),terrProb,explProb;
	if(nestProb>1) nestProb = 1;
	terrProb = (1.0 - nestProb)*(1 - EXPL);
	explProb = (1.0 - nestProb)*EXPL;

	vector<double> probs;
	vector<Tile> tiles;
	probs.push_back(nestProb);
	probs.push_back(explProb);
	probs.push_back(terrProb);
	tiles.push_back(n);
	tiles.push_back(e);
	tiles.push_back(t);

	int i = chooseBetweenProbs(probs);
	Tile& selected = tiles[i];
	char * desc[] = {"nest","expl","terr"};
	printf("Selector:\nnest:[%d,%d],%lf\nexpl:[%d,%d],%lf\nterr:[%d,%d],%lf\n",n.x,n.y,nestProb,e.x,e.y,explProb,t.x,t.y,terrProb);
	printf("selected %s\n",desc[i]);


	return getBestNeighbour(selected);
}

Tile Selector::getBestNeighbour(const Tile& dest)
{
	Tile my = Controller::instance()->getTile();
	vector<pair<Tile,double> > neighbours = OccupancyGrid::instance()->getNeighbors(my.x,my.y);
	int i=0,imin=0;
	double dmin = 100500;
	for(vector<pair<Tile,double> >::iterator it = neighbours.begin();it!=neighbours.end();it++,i++)
	{
		if(it->second>0.5)
			continue;
		double d = dist(it->first,dest);
		if(d<dmin)
		{
			dmin = d;
			imin = i;
		}
	}
	return neighbours[imin].first;

}

double Selector::dist(const Tile& a,const Tile& b)
{
	return sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y));

}

int Selector::chooseBetweenProbs(vector<double>& probs)
{
	int n = probs.size();
	double probSum = 0;
	double r = ((double)rand()/RAND_MAX);
	printf("rand=%lf",r);
    for(int i=0;i<n;i++)
    {
    	probSum += probs[i];
    	if(probSum > r)
    		return i;
    }
    throw -1;
}

